package org.noote.shaky.pad;

import java.io.IOException;
import java.util.ArrayList;
import java.util.concurrent.locks.Lock;
import java.util.concurrent.locks.ReentrantLock;

import org.noote.libs.robot.Robot_Skeleton;
import org.noote.libs.robot.Robot_ServoMotor;
import org.noote.libs.robot.stepper.StepperRobot_Animation;
import org.noote.libs.robot.stepper.StepperRobot_AnimationContext;
import org.noote.libs.robot.object3d.Robot_Bone3D;
import org.noote.libs.robot.object3d.Robot_Skeleton3D;
import org.noote.libs.robot.object3d.helper.Mesh_Servo;
import org.noote.libs.shaky.comm.BluetoothCommandProtocol;
import org.noote.libs.sogle.Material;
import org.noote.libs.sogle.Mesh;
import org.noote.libs.sogle.Vector3D;
import org.noote.libs.sogle.helper.SimpleMeshFormat_Loader;

import android.content.Context;

public class Shaky_Skeleton extends Robot_Skeleton3D {
	
	public static final int HEAD_NECK_1 = 0, HEAD_NECK_2 = 1;
	public static final int SHOULDER_LEFT = 2, BICEP_LEFT = 3, ELBOW_LEFT = 4, WRIST_LEFT = 5, HAND_LEFT = 5;
	public static final int SHOULDER_RIGHT = 7, BICEP_RIGHT = 8, ELBOW_RIGHT = 9, WRIST_RIGHT = 10,  HAND_RIGHT = 11;
	public static final int HIP_1_LEFT = 12, HIP_2_LEFT = 13, THIGH_LEFT = 14, KNEE_LEFT = 15, ANKLE_LEFT = 16;
	public static final int HIP_1_RIGHT= 17, HIP_2_RIGHT = 18, THIGH_RIGHT = 19, KNEE_RIGHT = 20, ANKLE_RIGHT = 21;
	
	ArrayList<StepperRobot_Animation> _anims = new ArrayList<StepperRobot_Animation>();
	private StepperRobot_Animation _anim_default = null;
	
	StepperRobot_AnimationContext _animContext = null;
	
	private final Lock lockUpdate = new ReentrantLock();

	Shaky_Skeleton() {
		super(new Robot_Skeleton());
		
		_animContext = new StepperRobot_AnimationContext(this._robot);
		
		addDefaultAnim(new Shaky_Anim_Right());
		
		//addAnim(new Shaky_Anim_Test());
		addAnim(new Shaky_Anim_Hello());
		addAnim(new Shaky_Anim_Walk1());
		addAnim(new Shaky_Anim_Hulk());
		addAnim(new Shaky_Anim_Pump());
		addAnim(new Shaky_Anim_SemiPump());
	}
	
	boolean addDefaultAnim(StepperRobot_Animation anim)
	{
		if(!addAnim(anim)) return false;
		_anim_default = anim;
		return true;
	}
	boolean addAnim(StepperRobot_Animation anim)
	{
		if(!anim.create()) return false;
		return _anims.add(anim);
	}
	
	void create(Context context)
	{
		Mesh_Servo mservo;
		Mesh mesh;
		Mesh meshAbs04=null, meshAbs09=null, meshAbs10=null, meshAbs11=null, meshAbs15=null, meshAbs16=null, meshArf02=null;
		Material matAbs = null;
		
		matAbs = new Material();
		matAbs.setAmbient(0.12f, 0.12f, 0.12f);
		matAbs.setDiffuse(0.25f, 0.25f, 0.25f);
		matAbs.setSpecular(64, 0.5f, 0.5f, 0.5f);
		try {
			meshAbs04 = SimpleMeshFormat_Loader.loadAsMesh(context.getAssets().open("abs-04.txt"), matAbs);
			meshAbs09 = SimpleMeshFormat_Loader.loadAsMesh(context.getAssets().open("abs-09 - c_bracket.txt"), matAbs);
			meshAbs10 = SimpleMeshFormat_Loader.loadAsMesh(context.getAssets().open("abs-10 - long_c_bracket.txt"), matAbs);
			meshAbs11 = SimpleMeshFormat_Loader.loadAsMesh(context.getAssets().open("abs-11 - offset_c.txt"), matAbs);
			meshAbs15 = SimpleMeshFormat_Loader.loadAsMesh(context.getAssets().open("abs-15 - dual_inline.txt"), matAbs);
			meshAbs16 = SimpleMeshFormat_Loader.loadAsMesh(context.getAssets().open("abs-16 - servo_offset_axis.txt"), matAbs);
			meshArf02 = SimpleMeshFormat_Loader.loadAsMesh(context.getAssets().open("arf-02 - foot.txt"), matAbs);
		} catch (IOException e) {
			e.printStackTrace();
			
			return;
		}
		
		////////////////////////
		// assemble all parts
		
		// buste !
		Robot_Bone3D buste = newBone();
		buste.set(new Vector3D(0, 0, 0), 1.0f);
		buste.createBox(10, 9.7f, 4.5f, new Vector3D(0, -0.5f, 0), 0, 0, 1);
		mservo = new Mesh_Servo(); // neck 1 servo
		mservo.setPosition(0f, 3.5f, -1.0f);
		mservo.setEulerRotation(0, 180f, 0f);
		buste.addMesh(mservo);
		mservo = new Mesh_Servo(); // left arm servo
		mservo.setPosition(3.8f, 1.7f, 1.2f);
		mservo.setEulerRotation(0, 0f, -90f);
		buste.addMesh(mservo);
		mservo = new Mesh_Servo(); // right arm servo
		mservo.setPosition(-3.8f, 1.7f, 1.2f);
		mservo.setEulerRotation(0, 0f, 90f);
		buste.addMesh(mservo);
		mservo = new Mesh_Servo(); // left leg servo
		mservo.setPosition(3.1f, -3.5f, 2.2f);
		mservo.setEulerRotation(90, 0f, 0f);
		buste.addMesh(mservo);
		mservo = new Mesh_Servo(); // right leg servo
		mservo.setPosition(-3.1f, -3.5f, 2.2f);
		mservo.setEulerRotation(90, 0f, 0f);
		buste.addMesh(mservo);
		
		// cou
		Robot_Bone3D cou = newBone();
		cou.setParent(buste);
		cou.set(new Vector3D(0, 5.2f, -1.0f), 1.0f);
		cou.createBox(5.2f, 2.5f, 5.2f, new Vector3D(0, 1.25f, -0.7f), 1, 0, 0);
		mservo = new Mesh_Servo(); // neck 2 servo
		mservo.setPosition(0.8f, 1.40f, -1.9f);
		mservo.setEulerRotation(0, 180f, 90f);
		cou.addMesh(mservo);
		mesh = new Mesh(meshAbs04);
		mesh.setPosition(0.1f, 1.6f, -0.8f);
		mesh.setEulerRotation(0, 90, 0);
		cou.addMesh(mesh);
		
		// tete
		Robot_Bone3D tete = newBone();
		tete.setParent(cou);
		tete.set(new Vector3D(2.7f, 1.40f, -2.0f), 1.0f);
		tete.createBox(5.0f, 5.0f, 2.4f, new Vector3D(-2.7f, 5.2f, 0), 1, 1, 0);
		mesh = new Mesh(meshAbs09);
		mesh.setPosition(-2.6f, 3.0f, 0);
		mesh.setEulerRotation(180, 0, 0);
		tete.addMesh(mesh);
	
		// bras gauche
		Robot_Bone3D epaule_gauche = newBone();
		epaule_gauche.setParent(buste);
		epaule_gauche.set(new Vector3D(5.6f, 1.7f, 1.2f), 1.0f);
		epaule_gauche.createBox(0.6f, 4.4f, 5.2f, new Vector3D(0.3f, 0.9f, 0), 1, 0, 0);
		mesh = new Mesh(meshAbs11);
		mesh.setPosition(0.1f, 0.0f, 0.0f);
		mesh.setEulerRotation(90, 0, -90);
		epaule_gauche.addMesh(mesh);
		
		Robot_Bone3D bicep_gauche = newBone();
		bicep_gauche.setParent(epaule_gauche);
		bicep_gauche.set(new Vector3D(2.8f, 1.8f, -2.4f), 1.0f);
		bicep_gauche.createBox(11.4f, 2.2f, 5.2f, new Vector3D(-4, 0, 2.6f), 1, 1, 0);
		mservo = new Mesh_Servo(); // bicep left servo
		mservo.setPosition(0.0f, 0.0f, 1.9f);
		mservo.setEulerRotation(90, 90f, 180f);
		bicep_gauche.addMesh(mservo);
		mservo = new Mesh_Servo(); // elbow left servo
		mservo.setPosition(-7.8f, 0.0f, 1.9f);
		mservo.setEulerRotation(90, -90f, 180f);
		bicep_gauche.addMesh(mservo);
		mesh = new Mesh(meshAbs15);
		mesh.setPosition(-3.9f, 0.0f, 2.6f);
		mesh.setEulerRotation(0, 180, 0);
		bicep_gauche.addMesh(mesh);
		
		Robot_Bone3D poignet_gauche = newBone();
		poignet_gauche.setParent(bicep_gauche);
		poignet_gauche.set(new Vector3D(-7.8f, 0f, 0.0f), 1.0f);
		poignet_gauche.createBox(3.4f, 2.2f, 5.2f, new Vector3D(-3.8f, 0, 2.6f), 0, 1, 1);
		mservo = new Mesh_Servo(); // wrist left servo
		mservo.setPosition(-5.0f, 0.0f, 3.5f);
		mservo.setEulerRotation(0, 0f, 90);
		poignet_gauche.addMesh(mservo);
		mesh = new Mesh(meshAbs16);
		mesh.setPosition(0.0f, 0.0f, 2.6f);
		mesh.setEulerRotation(-90, 0, 90);
		poignet_gauche.addMesh(mesh);
		
		Robot_Bone3D main_gauche = newBone();
		main_gauche.setParent(poignet_gauche);
		main_gauche.set(new Vector3D(-6.8f, 0.0f, 3.5f), 1.0f);
		main_gauche.createBox(4.0f, 2.2f, 5.2f, new Vector3D(-2.0f, 0, 0), 0, 1, 0);
		mesh = new Mesh(meshAbs09);
		mesh.setPosition(0.0f, 0.0f, 0.0f);
		mesh.setEulerRotation(90, 0, 90);
		main_gauche.addMesh(mesh);
		
		// bras droit
		Robot_Bone3D epaule_droite = newBone();
		epaule_droite.setParent(buste);
		epaule_droite.set(new Vector3D(-5.6f, 1.7f, 1.2f), 1.0f);
		epaule_droite.createBox(0.6f, 4.4f, 5.2f, new Vector3D(-0.3f, 0.9f, 0), 1, 0, 0);
		mesh = new Mesh(meshAbs11);
		mesh.setPosition(-0.1f, 0.0f, 0.0f);
		mesh.setEulerRotation(90, 0, 90);
		epaule_droite.addMesh(mesh);
		
		Robot_Bone3D bicep_droit = newBone();
		bicep_droit.setParent(epaule_droite);
		bicep_droit.set(new Vector3D(-2.8f, 1.8f, -2.4f), 1.0f);
		bicep_droit.createBox(11.4f, 2.2f, 5.2f, new Vector3D(-4, 0, 2.6f), 1, 1, 0);
		mservo = new Mesh_Servo(); // bicep right servo
		mservo.setPosition(0.0f, 0.0f, 1.9f);
		mservo.setEulerRotation(90, 90f, 180f);
		bicep_droit.addMesh(mservo);
		mservo = new Mesh_Servo(); // elbow right servo
		mservo.setPosition(-7.8f, 0.0f, 1.9f);
		mservo.setEulerRotation(90, -90f, 180f);
		bicep_droit.addMesh(mservo);
		mesh = new Mesh(meshAbs15);
		mesh.setPosition(-3.9f, 0.0f, 2.6f);
		mesh.setEulerRotation(0, 180, 180);
		bicep_droit.addMesh(mesh);
		
		Robot_Bone3D poignet_droit = newBone();
		poignet_droit.setParent(bicep_droit);
		poignet_droit.set(new Vector3D(-7.8f, 0f, 0f), 1.0f);
		poignet_droit.createBox(3.4f, 2.2f, 5.2f, new Vector3D(-3.8f, 0, 2.6f), 0, 1, 1);
		mservo = new Mesh_Servo(); // wrist right servo
		mservo.setPosition(-5.0f, 0.0f, 3.5f);
		mservo.setEulerRotation(0, 0f, 90f);
		poignet_droit.addMesh(mservo);
		mesh = new Mesh(meshAbs16);
		mesh.setPosition(0.0f, 0.0f, 2.6f);
		mesh.setEulerRotation(90, 0, 90);
		poignet_droit.addMesh(mesh);
		
		Robot_Bone3D main_droite = newBone();
		main_droite.setParent(poignet_droit);
		main_droite.set(new Vector3D(-6.8f, 0.0f, 3.5f), 1.0f);
		main_droite.createBox(4.0f, 2.2f, 5.2f, new Vector3D(-2.0f, 0, 0), 0, 1, 0);
		mesh = new Mesh(meshAbs09);
		mesh.setPosition(0.0f, 0.0f, 0.0f);
		mesh.setEulerRotation(90, 0, 90);
		main_droite.addMesh(mesh);
		
		// jambe gauche
		Robot_Bone3D hanche_1_gauche = newBone();
		hanche_1_gauche.setParent(buste);
		hanche_1_gauche.set(new Vector3D(3.1f, -3.6f, 1.5f), 1.0f);
		hanche_1_gauche.createBox(2.4f, 4.0f, 5.2f, new Vector3D(0, -4.3f, 0), 1, 0, 0);
		mservo = new Mesh_Servo(); // hip 2 left servo
		mservo.setPosition(0f, -5.0f, -1.0f);
		mservo.setEulerRotation(0, 180f, 180f);
		hanche_1_gauche.addMesh(mservo);
		mesh = new Mesh(meshAbs16);
		mesh.setPosition(0.0f, 0.0f, 0.0f);
		mesh.setEulerRotation(-90, -90, 90);
		hanche_1_gauche.addMesh(mesh);
		
		Robot_Bone3D hanche_2_gauche = newBone();
		hanche_2_gauche.setParent(hanche_1_gauche);
		hanche_2_gauche.set(new Vector3D(0f, -6.8f, -1.0f), 1.0f);
		hanche_2_gauche.createBox(5.2f, 0.2f, 2.2f, new Vector3D(0, -0.2f, 0), 1, 1, 0);
		mesh = new Mesh(meshAbs09);
		mesh.setPosition(0.0f, -0.1f, 0.0f);
		mesh.setEulerRotation(180, 0, 0);
		hanche_2_gauche.addMesh(mesh);
		
		Robot_Bone3D cuisse_gauche = newBone();
		cuisse_gauche.setParent(hanche_2_gauche);
		cuisse_gauche.set(new Vector3D(-2.4f, -3.0f, 0f), 1.0f);
		cuisse_gauche.createBox(4.8f, 11.4f, 2.2f, new Vector3D(2.4f, 4.0f, 0), 0, 1, 1);
		mservo = new Mesh_Servo(); // thigh left servo
		mservo.setPosition(1.8f, 0.0f, 0f);
		mservo.setEulerRotation(90, 0f, 90f);
		cuisse_gauche.addMesh(mservo);
		mservo = new Mesh_Servo(); // knee left servo
		mservo.setPosition(1.8f, 5.8f, 0f);
		mservo.setEulerRotation(90, 0f, 90f);
		cuisse_gauche.addMesh(mservo);
		mesh = new Mesh(meshAbs15);
		mesh.setPosition(2.5f, 4.0f, 0.0f);
		mesh.setEulerRotation(180, -90, 90);
		cuisse_gauche.addMesh(mesh);
		
		Robot_Bone3D tibia_gauche = newBone();
		tibia_gauche.setParent(cuisse_gauche);
		tibia_gauche.set(new Vector3D(0f, 5.8f, 0f), 1.0f);
		tibia_gauche.createBox(2.2f, 2.2f, 2.2f, new Vector3D(2.4f, 6.0f, 0), 0, 1, 0);
		mesh = new Mesh(meshAbs10);
		mesh.setPosition(2.6f, 4.5f, 0.0f);
		mesh.setEulerRotation(180, 0, 0);
		tibia_gauche.addMesh(mesh);
		mesh = new Mesh(meshAbs10);
		mesh.setPosition(2.6f, 4.7f, 0.0f);
		mesh.setEulerRotation(0, 90, 0);
		tibia_gauche.addMesh(mesh);
		
		Robot_Bone3D pied_gauche = newBone();
		pied_gauche.setParent(tibia_gauche);
		pied_gauche.set(new Vector3D(2.4f, 9.2f, 2.7f), 1.0f);
		pied_gauche.createBox(7.8f, 2.5f, 12.8f, new Vector3D(2.4f, 0.0f, -4.7f), 1, 0, 1);
		mservo = new Mesh_Servo(); // ankle left servo
		mservo.setPosition(0.0f, 0.0f, -1.9f);
		mservo.setEulerRotation(180, -90f, 90f);
		pied_gauche.addMesh(mservo);
		mesh = new Mesh(meshArf02);
		mesh.setPosition(2.4f, 1.2f, -2.4f);
		mesh.setEulerRotation(180, 0, 0);
		pied_gauche.addMesh(mesh);
		
		// jambe droite
		Robot_Bone3D hanche_1_droite = newBone();
		hanche_1_droite.setParent(buste);
		hanche_1_droite.set(new Vector3D(-3.1f, -3.6f, 1.5f), 1.0f);
		hanche_1_droite.createBox(2.4f, 4.0f, 5.2f, new Vector3D(0, -4.3f, 0), 1, 0, 0);
		mservo = new Mesh_Servo(); // hip 2 right servo
		mservo.setPosition(0f, -5.0f, -1.0f);
		mservo.setEulerRotation(0, 180f, 180f);
		hanche_1_droite.addMesh(mservo);
		mesh = new Mesh(meshAbs16);
		mesh.setPosition(0.0f, 0.0f, 0.0f);
		mesh.setEulerRotation(90, 90, 90);
		hanche_1_droite.addMesh(mesh);
		
		Robot_Bone3D hanche_2_droite = newBone();
		hanche_2_droite.setParent(hanche_1_droite);
		hanche_2_droite.set(new Vector3D(0f, -6.8f, -1.0f), 1.0f);
		hanche_2_droite.createBox(5.2f, 0.2f, 2.2f, new Vector3D(0, -0.2f, 0), 1, 1, 0);
		mesh = new Mesh(meshAbs09);
		mesh.setPosition(0.0f, -0.1f, 0.0f);
		mesh.setEulerRotation(180, 0, 0);
		hanche_2_droite.addMesh(mesh);
		
		Robot_Bone3D cuisse_droite = newBone();
		cuisse_droite.setParent(hanche_2_droite);
		cuisse_droite.set(new Vector3D(2.4f, -3.0f, 0f), 1.0f);
		cuisse_droite.createBox(4.8f, 11.4f, 2.2f, new Vector3D(-2.4f, 4.0f, 0), 0, 1, 1);
		mservo = new Mesh_Servo(); // thigh right servo
		mservo.setPosition(-1.8f, 0.0f, 0f);
		mservo.setEulerRotation(90, 0f, -90f);
		cuisse_droite.addMesh(mservo);
		mservo = new Mesh_Servo(); // knee right servo
		mservo.setPosition(-1.8f, 5.8f, 0f);
		mservo.setEulerRotation(90, 0f, -90f);
		cuisse_droite.addMesh(mservo);
		mesh = new Mesh(meshAbs15);
		mesh.setPosition(-2.5f, 4.0f, 0.0f);
		mesh.setEulerRotation(0, 90, 90);
		cuisse_droite.addMesh(mesh);
		
		Robot_Bone3D tibia_droit = newBone();
		tibia_droit.setParent(cuisse_droite);
		tibia_droit.set(new Vector3D(0f, 5.8f, 0f), 1.0f);
		tibia_droit.createBox(2.2f, 2.2f, 2.2f, new Vector3D(-2.4f, 6.0f, 0), 0, 1, 0);
		mesh = new Mesh(meshAbs10);
		mesh.setPosition(-2.6f, 4.5f, 0.0f);
		mesh.setEulerRotation(180, 0, 0);
		tibia_droit.addMesh(mesh);
		mesh = new Mesh(meshAbs10);
		mesh.setPosition(-2.6f, 4.7f, 0.0f);
		mesh.setEulerRotation(0, 90, 0);
		tibia_droit.addMesh(mesh);
		
		Robot_Bone3D pied_droit = newBone();
		pied_droit.setParent(tibia_droit);
		pied_droit.set(new Vector3D(-2.4f, 9.2f, 2.7f), 1.0f);
		pied_droit.createBox(7.8f, 2.5f, 12.8f, new Vector3D(-2.4f, 0.0f, -4.7f), 1, 0, 1);
		mservo = new Mesh_Servo(); // ankle right servo
		mservo.setPosition(0.0f, 0.0f, -1.9f);
		mservo.setEulerRotation(0, 90f, 90f);
		pied_droit.addMesh(mservo);
		mesh = new Mesh(meshArf02);
		mesh.setPosition(-2.4f, 1.2f, -2.4f);
		mesh.setEulerRotation(180, 0, 0);
		pied_droit.addMesh(mesh);
		
		//////////////////////////////////////////
		// create servos and associate to bones
		Robot_ServoMotor head_neck_1 = _robot.newServo(HEAD_NECK_1, 0, 90, 180, 0.21f/60.0f, false);
		head_neck_1.setAxis(90, new Vector3D(0, 1, 0));
		head_neck_1.setBone(cou);
		
		Robot_ServoMotor head_neck_2 = _robot.newServo(HEAD_NECK_2, 0, 90, 180, 0.21f/60.0f, false);
		head_neck_2.setAxis(-90, new Vector3D(1, 0, 0));
		head_neck_2.setBone(tete);
		
		// bras gauche
		Robot_ServoMotor shoulder_left = _robot.newServo(SHOULDER_LEFT, 0, 90, 180, 0.21f/60.0f, false);
		shoulder_left.setAxis(-30, new Vector3D(1, 0, 0));
		shoulder_left.setBone(epaule_gauche);
		
		Robot_ServoMotor bicep_left = _robot.newServo(BICEP_LEFT, 0, 90, 180, 0.21f/60.0f, false);
		bicep_left.setAxis(90, new Vector3D(0, 0, 1));
		bicep_left.setBone(bicep_gauche);
		
		Robot_ServoMotor elbow_left = _robot.newServo(ELBOW_LEFT, 0, 90, 180, 0.21f/60.0f, false);
		elbow_left.setAxis(180, new Vector3D(0, 0, -1));
		elbow_left.setBone(poignet_gauche);
		
		Robot_ServoMotor wrist_left = _robot.newServo(WRIST_LEFT, 0, 90, 180, 0.21f/60.0f, false);
		wrist_left.setAxis(90, new Vector3D(-1, 0, 0));
		wrist_left.setBone(main_gauche);
		
		// bras droit
		Robot_ServoMotor shoulder_right = _robot.newServo(SHOULDER_RIGHT, 0, 90, 180, 0.21f/60.0f, false);
		shoulder_right.setAxis(180+30, new Vector3D(-1, 0, 0));
		shoulder_right.setBone(epaule_droite);
		
		Robot_ServoMotor bicep_right = _robot.newServo(BICEP_RIGHT, 0, 90, 180, 0.21f/60.0f, false);
		bicep_right.setAxis(180+90, new Vector3D(0, 0, 1));
		bicep_right.setBone(bicep_droit);
		
		Robot_ServoMotor elbow_right = _robot.newServo(ELBOW_RIGHT, 0, 90, 180, 0.21f/60.0f, false);
		elbow_right.setAxis(0, new Vector3D(0, 0, -1));
		elbow_right.setBone(poignet_droit);
		
		Robot_ServoMotor wrist_right = _robot.newServo(WRIST_RIGHT, 0, 90, 180, 0.21f/60.0f, false);
		wrist_right.setAxis(90, new Vector3D(-1, 0, 0));
		wrist_right.setBone(main_droite);
		
		// jambe gauche
		Robot_ServoMotor hip_1_left = _robot.newServo(HIP_1_LEFT, 0, 90, 180, 0.16f/60.0f, false);
		hip_1_left.setAxis(-90, new Vector3D(0, 0, -1));
		hip_1_left.setBone(hanche_1_gauche);
		
		Robot_ServoMotor hip_2_left = _robot.newServo(HIP_2_LEFT, 0, 90, 180, 0.16f/60.0f, false);
		hip_2_left.setAxis(-90, new Vector3D(0, 1, 0));
		hip_2_left.setBone(hanche_2_gauche);
		
		Robot_ServoMotor thigh_left = _robot.newServo(THIGH_LEFT, 0, 90, 180, 0.16f/60.0f, false);
		thigh_left.setAxis(90, new Vector3D(-1, 0, 0));
		thigh_left.setBone(cuisse_gauche);
		
		Robot_ServoMotor knee_left = _robot.newServo(KNEE_LEFT, 0, 90, 180, 0.16f/60.0f, false);
		knee_left.setAxis(-90, new Vector3D(1, 0, 0));
		knee_left.setBone(tibia_gauche);
		
		Robot_ServoMotor ankle_left = _robot.newServo(ANKLE_LEFT, 0, 90, 180, 0.16f/60.0f, false);
		ankle_left.setAxis(-90, new Vector3D(0, 0, 1));
		ankle_left.setBone(pied_gauche);
		
		// jambe droite
		Robot_ServoMotor hip_1_right = _robot.newServo(HIP_1_RIGHT, 0, 90, 180, 0.16f/60.0f, false);
		hip_1_right.setAxis(-90, new Vector3D(0, 0, -1));
		hip_1_right.setBone(hanche_1_droite);
		
		Robot_ServoMotor hip_2_right = _robot.newServo(HIP_2_RIGHT, 0, 90, 180, 0.16f/60.0f, false);
		hip_2_right.setAxis(-90, new Vector3D(0, 1, 0));
		hip_2_right.setBone(hanche_2_droite);
		
		Robot_ServoMotor thigh_right  = _robot.newServo(THIGH_RIGHT, 0, 90, 180, 0.16f/60.0f, false);
		thigh_right.setAxis(90, new Vector3D(1, 0, 0));
		thigh_right.setBone(cuisse_droite);
		
		Robot_ServoMotor knee_right = _robot.newServo(KNEE_RIGHT, 0, 90, 180, 0.16f/60.0f, false);
		knee_right.setAxis(-90, new Vector3D(-1, 0, 0));
		knee_right.setBone(tibia_droit);
		
		Robot_ServoMotor ankle_right = _robot.newServo(ANKLE_RIGHT, 0, 90, 180, 0.16f/60.0f, false);
		ankle_right.setAxis(-90, new Vector3D(0, 0, 1));
		ankle_right.setBone(pied_droit);
		
		setDefaultPosition();
	}
	
	boolean setDefaultPosition()
	{
		if(_anim_default == null) return false;
		_animContext.start(_anim_default);
		
		update();
		
		return true;
	}
	
	void setAnimation(int iAnim)
	{
		if(iAnim>=0 && iAnim<=_anims.size())
		{
			_animContext.start(_anims.get(iAnim));
			for(Robot_ServoMotor s : _robot.getServoList())
			{
				s.flagUntransmitted();
			}
		}
	}
	public boolean isAnimationFinish() {
		return _animContext.isFinished();
	}

	public void doAnimation(float fTimeSlice) {
		_animContext.addTime(fTimeSlice);
	}
	
	
	@Override
	public void update()
	{
		if(lockUpdate.tryLock())
		{
			try
			{
				super.update();
			}
			finally
			{
				lockUpdate.unlock();
			}
		}
	}
	
	public String buildTransmitMessage()
	{
		int nServos = 0;
		int servos_id[] = new int[_robot.getServoList().size()];
		int servos_pos[] = new int[servos_id.length];
		
		if(lockUpdate.tryLock())
		{
			try
			{
				for(Robot_ServoMotor s : _robot.getServoList())
				{
					if(s.isTransmittedHaveChangedPosition())
					{
						servos_id[nServos] = s.getID();
						servos_pos[nServos] = (int)s.getRealPosition(); // TODO
						s.flagTransmitted();
						nServos++;
					}
				}
			}
			finally
			{
				lockUpdate.unlock();
			}
		}
		
		if(nServos>0)
			return BluetoothCommandProtocol.sendServoPositions(0, nServos, servos_id, servos_pos);
		
		return "";
	}
}
